/*
 * @Author: Jiagang Chen
 * @Date: 2021-11-15 03:34:23
 * @LastEditors: Jiagang Chen
 * @LastEditTime: 2021-12-07 09:21:47
 * @Description:
 * @Reference:
 */

#ifndef _VILENS_HELP_H_
#define _VILENS_HELP_H_

#include "lci_slam/vilens/ceres/IntegrationBase.h"
#include "lci_slam/vilens/common_header.h"
#include "lci_slam/vilens/utility/utility.h"

namespace vilens {

struct icpEstimation {
    typedef std::shared_ptr<icpEstimation> Ptr;

    int windowSize;
    std::vector<Eigen::Vector3d> imuPos;
    std::vector<Eigen::Matrix3d> imuRot;
    std::vector<Eigen::Vector3d> imuVel;
    std::vector<Eigen::Vector3d> imuAccBias;
    std::vector<Eigen::Vector3d> imuGyrBias;
    Eigen::Matrix4d relPose;

    double **para_Pose;
    double **para_SpeedBias;
    double *para_Ex_Pose;

    Eigen::Vector3d origin_R0;
    Eigen::Vector3d origin_P0;

    icpEstimation(const int size) {
        windowSize = size;
        imuPos.resize(windowSize);
        imuRot.resize(windowSize);
        imuVel.resize(windowSize);
        imuAccBias.resize(windowSize);
        imuGyrBias.resize(windowSize);

        para_Pose = new double *[windowSize];
        para_SpeedBias = new double *[windowSize];
        for (int i = 0; i < windowSize; i++) {
            para_Pose[i] = new double[7];
            para_SpeedBias[i] = new double[9];
        }
        para_Ex_Pose = new double[7];

        origin_R0 = Utility::R2ypr(imuRot[0]);
        origin_P0 = imuPos[0];
    }

    ~icpEstimation() {}

    void vector2double() {
        for (int i = 0; i < windowSize; i++) {
            para_Pose[i][0] = imuPos[i].x();
            para_Pose[i][1] = imuPos[i].y();
            para_Pose[i][2] = imuPos[i].z();
            Eigen::Quaterniond q{imuRot[i]};
            para_Pose[i][3] = q.x();
            para_Pose[i][4] = q.y();
            para_Pose[i][5] = q.z();
            para_Pose[i][6] = q.w();

            para_SpeedBias[i][0] = imuVel[i].x();
            para_SpeedBias[i][1] = imuVel[i].y();
            para_SpeedBias[i][2] = imuVel[i].z();

            para_SpeedBias[i][3] = imuAccBias[i].x();
            para_SpeedBias[i][4] = imuAccBias[i].y();
            para_SpeedBias[i][5] = imuAccBias[i].z();

            para_SpeedBias[i][6] = imuGyrBias[i].x();
            para_SpeedBias[i][7] = imuGyrBias[i].y();
            para_SpeedBias[i][8] = imuGyrBias[i].z();
        }

        para_Ex_Pose[0] = relPose(0, 3);
        para_Ex_Pose[1] = relPose(1, 3);
        para_Ex_Pose[2] = relPose(2, 3);
        Quaterniond q{relPose.block<3, 3>(0, 0)};
        para_Ex_Pose[3] = q.x();
        para_Ex_Pose[4] = q.y();
        para_Ex_Pose[5] = q.z();
        para_Ex_Pose[6] = q.w();
    }

    void double2vector() {
        Eigen::Vector3d origin_R00 =
            Utility::R2ypr(Quaterniond(para_Pose[0][6], para_Pose[0][3],
                                       para_Pose[0][4], para_Pose[0][5])
                               .toRotationMatrix());
        double yawDiff = origin_R0.x() - origin_R00.x();
        Eigen::Matrix3d rotDiff =
            Utility::ypr2R(Eigen::Vector3d(yawDiff, 0, 0));
        if (std::abs(std::abs(origin_R0.y()) - 90) < 1.0 ||
            std::abs(std::abs(origin_R00.y()) - 90) < 1.0) {
            rotDiff = imuRot[0] * Quaterniond(para_Pose[0][6], para_Pose[0][3],
                                              para_Pose[0][4], para_Pose[0][5])
                                      .toRotationMatrix()
                                      .transpose();
        }

        for (int i = 0; i < windowSize; i++) {
            imuRot[i] =
                rotDiff * Eigen::Quaterniond(para_Pose[i][6], para_Pose[i][3],
                                             para_Pose[i][4], para_Pose[i][5])
                              .normalized()
                              .toRotationMatrix();

            imuPos[i] =
                rotDiff * Eigen::Vector3d(para_Pose[i][0] - para_Pose[0][0],
                                          para_Pose[i][1] - para_Pose[0][1],
                                          para_Pose[i][2] - para_Pose[0][2]) +
                origin_P0;

            imuVel[i] = rotDiff * Eigen::Vector3d(para_SpeedBias[i][0],
                                                  para_SpeedBias[i][1],
                                                  para_SpeedBias[i][2]);

            imuAccBias[i] =
                Eigen::Vector3d(para_SpeedBias[i][3], para_SpeedBias[i][4],
                                para_SpeedBias[i][5]);

            imuGyrBias[i] =
                Eigen::Vector3d(para_SpeedBias[i][6], para_SpeedBias[i][7],
                                para_SpeedBias[i][8]);
        }

        relPose.block<3, 1>(0, 3) =
            Eigen::Vector3d(para_Ex_Pose[0], para_Ex_Pose[1], para_Ex_Pose[2]);
        relPose.block<3, 3>(0, 0) =
            Eigen::Quaterniond(para_Ex_Pose[6], para_Ex_Pose[3],
                               para_Ex_Pose[4], para_Ex_Pose[5])
                .normalized()
                .toRotationMatrix();
    }
};

struct imuMeasurements {
    typedef std::shared_ptr<imuMeasurements> Ptr;
    std::vector<sensor_msgs::Imu::Ptr> imus;
    Eigen::Vector3d accBias;
    Eigen::Vector3d gyroBias;
    double prevTime;
    double currTime;
    IntegrationBase::Ptr preIntegration;

    // accl.size(gyro.size()) == imus.size() + 2
    bool haveDiscretized = false;
    std::vector<Eigen::Vector3d> accl;
    std::vector<Eigen::Vector3d> gyro;
    // accl.size(gyro.size()) == imus.size() + 1
    std::vector<double> dt;

    imuMeasurements::Ptr operator+(const imuMeasurements::Ptr &b) {
        if (this->prevTime != b->currTime || this->currTime != b->prevTime) {
            LOG(WARNING) << "this->prevTime != b->currTime || this->currTime "
                            "!= b->prevTime";
            return nullptr;
        }

        imuMeasurements::Ptr a(new imuMeasurements());
        if (this->prevTime == b->currTime) {
            a->imus = b->imus;
            a->imus.insert(a->imus.end(), this->imus.begin(), this->imus.end());
            a->prevTime = b->prevTime;
            a->currTime = this->currTime;
        } else {
            a->imus = this->imus;
            a->imus.insert(a->imus.end(), b->imus.begin(), b->imus.end());
            a->prevTime = this->prevTime;
            a->currTime = b->currTime;
        }

        a->accBias = (this->accBias + b->accBias) / 2.0;
        a->gyroBias = (this->gyroBias + b->gyroBias) / 2.0;
        a->haveDiscretized = false;
    }

    bool discretization() {
        if (imus.size() == 0) {
            LOG(WARNING) << "discretization : imus.size() == 0";
            return false;
        }

        accl.resize(imus.size() + 2);
        gyro.resize(imus.size() + 2);
        dt.resize(imus.size() + 1);

        double prev_ts = -1;
        Eigen::Vector3d prev_accl;
        Eigen::Vector3d prev_gyro;
        for (int i = 0; i < imus.size(); i++) {
            const auto &imu = imus[i];
            double ts = imu->header.stamp.toSec();
            Eigen::Vector3d curr_accl = Eigen::Vector3d(
                imu->linear_acceleration.x, imu->linear_acceleration.y,
                imu->linear_acceleration.z);
            Eigen::Vector3d curr_gyro = Eigen::Vector3d(
                imu->angular_velocity.x, imu->angular_velocity.y,
                imu->angular_velocity.z);
            if (i == 0) {
                dt[i] = ts - prevTime;
                accl[i] = curr_accl;
                gyro[i] = curr_gyro;
            } else {
                dt[i] = ts - prev_ts;
                accl[i] = prev_accl;
                gyro[i] = prev_gyro;
            }

            prev_ts = ts;
            prev_accl = curr_accl;
            prev_gyro = curr_gyro;
        }

        dt[imus.size()] = currTime - prev_ts;
        accl[imus.size()] = prev_accl;
        gyro[imus.size()] = prev_gyro;
        accl[imus.size() + 1] = prev_accl;
        gyro[imus.size() + 1] = prev_gyro;
        haveDiscretized = true;
        preIntegration = nullptr;

        return true;
    }

    bool preintegration() {
        if (!haveDiscretized) {
            LOG(WARNING) << "preintegration : haveDiscretized is false";
            return false;
        }

        preIntegration.reset(
            new IntegrationBase(accl[0], gyro[0], accBias, gyroBias));
        for (int i = 0; i < (int)dt.size(); i++) {
            preIntegration->push_back(dt[i], accl[1 + i], gyro[1 + i]);
        }

        return true;
    }

    bool integration(const Eigen::Vector3d &P0, const Eigen::Vector3d &V0,
                     const Eigen::Matrix3d &R0, const Eigen::Vector3d &gw,
                     Eigen::Vector3d &P1, Eigen::Vector3d &V1,
                     Eigen::Matrix3d &R1) {
        if (!haveDiscretized) {
            LOG(WARNING) << "integration : haveDiscretized is false";
            return false;
        }

        Eigen::Vector3d Ptemp = P0, Vtemp = V0;
        Eigen::Matrix3d Rtemp = R0;
        for (int i = 0; i < (int)dt.size(); i++) {
            const double &deltaT = dt[i];
            Eigen::Vector3d un_acc0 = Rtemp * (accl[i] - accBias) - gw;
            Eigen::Vector3d un_gyro = 0.5 * (gyro[i] + gyro[i + 1]) - gyroBias;
            Rtemp = Utility::deltaQ(un_gyro * deltaT).toRotationMatrix();
            Eigen::Vector3d un_acc1 = Rtemp * (accl[i + 1] - accBias) - gw;
            Eigen::Vector3d un_acc = 0.5 * (un_acc0 + un_acc1);
            Ptemp = Ptemp + Vtemp * deltaT + 0.5 * un_acc * deltaT * deltaT;
            Vtemp = Vtemp + un_acc * deltaT;
        }

        P1 = Ptemp;
        V1 = Vtemp;
        R1 = Rtemp;

        return true;
    }
};

inline void pointTransform(const PointType &pi, PointType &po,
                           const Eigen::Matrix4d &transform) {
    Eigen::Vector4d p_in(pi.x, pi.y, pi.z, 1.0);
    Eigen::Vector4d p_out = transform * p_in;

    po = pi;
    po.x = p_out(0);
    po.y = p_out(1);
    po.z = p_out(2);
}

inline void pointCloudTransform(const pcl::PointCloud<PointType>::Ptr &pcli,
                                pcl::PointCloud<PointType>::Ptr &pclo,
                                const Eigen::Matrix4d &transform) {
    int point_cloud_size = pcli->size();
    pclo->resize(point_cloud_size);
    for (int i = 0; i < point_cloud_size; i++) {
        pointTransform(pcli->points[i], pclo->points[i], transform);
    }
}

inline float pointDistance(const PointType &pi, const PointType &po) {
    return std::sqrt((pi.x - po.x) * (pi.x - po.x) +
                     (pi.y - po.y) * (pi.y - po.y) +
                     (pi.z - po.z) * (pi.z - po.z));
}

inline cv::Mat getImageFromMsg(const sensor_msgs::ImageConstPtr &img_msg) {
    cv_bridge::CvImageConstPtr ptr;
    if (img_msg->encoding == "8UC1") {
        sensor_msgs::Image img;
        img.header = img_msg->header;
        img.height = img_msg->height;
        img.width = img_msg->width;
        img.is_bigendian = img_msg->is_bigendian;
        img.step = img_msg->step;
        img.data = img_msg->data;
        img.encoding = "mono8";
        ptr = cv_bridge::toCvCopy(img, sensor_msgs::image_encodings::MONO8);
    } else
        ptr = cv_bridge::toCvCopy(img_msg, sensor_msgs::image_encodings::MONO8);

    cv::Mat img = ptr->image.clone();
    return img;
}

inline double AngularDistance(const Eigen::Matrix3d &rota,
                              const Eigen::Matrix3d &rotb) {
    double norm = ((rota * rotb.transpose()).trace() - 1) / 2;
    norm = std::acos(norm) * 180 / M_PI;
    return norm;
}

inline std::vector<int> getRamdomIndex(int index_size, int ramdomnumber,
                                       unsigned int realRamdom = std::time(0)) {
    std::vector<int> ramdomIndex(index_size);
    for (int i = 0; i < index_size; i++) {
        ramdomIndex[i] = i;
    }
    if (index_size > ramdomnumber) {
        std::srand(realRamdom);
        std::random_shuffle(ramdomIndex.begin(), ramdomIndex.end());
        ramdomIndex.resize(ramdomnumber);
    }

    return ramdomIndex;
}

}  // namespace vilens

#endif